// server.cpp
#include "websocket_client.h"
#include "psdk_controller.h"
#include <iostream>
#include <thread>
#include <string>
#include <queue>
#include <mutex>
#include <condition_variable>
#include <chrono>
#include <cstring>
#include "json.hpp"
#include "dji_gimbal_manager.h"
#include <algorithm>
#include <cctype>
#include <fcntl.h>
#include <sys/stat.h>
#include <unistd.h>
#include <errno.h>
#include <atomic>
#include <sstream>

const std::string SERVER_URL = "ws://192.168.1.111:8081/ws/psdk-client-M350";

// 使用 nlohmann::json
using json = nlohmann::json;

// 命令队列和互斥锁
static std::queue<std::string> cmd_queue;
static std::mutex cmd_mutex;
static std::condition_variable cmd_cv;
static bool should_exit = false;

// ROS(管道)控制启用状态标志
static std::atomic<bool> g_is_ros_control_enabled(false);

// 命名管道的路径
const char* PIPE_READ_PATH = "/tmp/psdk_pipe_read";
const char* PIPE_WRITE_PATH = "/tmp/psdk_pipe_write";
static int pipe_read_fd = -1;
static int pipe_write_fd = -1;

// 队列最大长度
static const size_t MAX_QUEUE_SIZE = 100;

// 用于等待飞行器静止的常量和函数
static const float VELOCITY_THRESHOLD = 0.1f; // 认为飞行器静止的速度阈值 (m/s)
static const int CHECK_INTERVAL_MS = 100;     // 检查速度的时间间隔 (ms)
static const int MAX_CHECK_TIME_MS = 30000;   // 最大等待时间 (ms)

// 全局变量，用于存储原点坐标
static std::atomic<double> g_origin_lat(0.0);
static std::atomic<double> g_origin_lon(0.0);
static std::atomic<bool> g_origin_is_set(false);

static std::atomic<double> g_target_x(0.0);
static std::atomic<double> g_target_y(0.0);
static std::atomic<double> g_target_z(0.0);

static bool wait_until_aircraft_stationary() {
    auto start_time = std::chrono::steady_clock::now();
    
    while (true) {
        float vx = 0, vy = 0, vz = 0;
        psdk_get_velocity(&vx, &vy, &vz);
        
        // 检查三个方向的速度是否都小于阈值
        if (std::abs(vx) < VELOCITY_THRESHOLD && 
            std::abs(vy) < VELOCITY_THRESHOLD && 
            std::abs(vz) < VELOCITY_THRESHOLD) {
            std::cout << "[状态检查] 飞行器已静止。" << std::endl;
            return true;
        }
        
        // 检查是否超时
        auto current_time = std::chrono::steady_clock::now();
        auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
            current_time - start_time).count();
        if (elapsed >= MAX_CHECK_TIME_MS) {
            std::cout << "[状态检查] 等待飞行器静止超时。" << std::endl;
            return false;
        }
        
        std::this_thread::sleep_for(std::chrono::milliseconds(CHECK_INTERVAL_MS));
    }
}

// 处理WebSocket消息的回调函数 
static void handle_websocket_message(const char* message, int len) {
    std::string cmd(message, len);
    try {
        json cmd_json = json::parse(cmd);
        std::string cmd_type = cmd_json.value("command", "");
        
        // 检查是否是高频控制命令
        bool is_realtime = (cmd_type == "vstick_advance_v" || 
                           cmd_type == "vstick_advance_p" || 
                           cmd_type == "set_ros_x_y_z" || 
                           cmd_type == "position_control");
        
        {
            std::lock_guard<std::mutex> lock(cmd_mutex);
            
            // 队列长度检查
            if (cmd_queue.size() >= MAX_QUEUE_SIZE) {
                // 丢弃最旧命令
                cmd_queue.pop();
            }
            
            // 对于高频控制命令，移除队列中同类型的旧命令
            if (is_realtime) {
                std::queue<std::string> new_queue;
                while (!cmd_queue.empty()) {
                    std::string front_cmd = cmd_queue.front();
                    cmd_queue.pop();
                    
                    bool keep_command = true;
                    try {
                        json front_json = json::parse(front_cmd);
                        std::string front_type = front_json.value("command", "");
                        if (front_type == cmd_type) {
                            keep_command = false; // 移除同类型旧命令
                        }
                    } catch (...) {
                        // 非JSON命令保留
                    }
                    
                    if (keep_command) {
                        new_queue.push(front_cmd);
                    }
                }
                cmd_queue = new_queue;
            }
            
            cmd_queue.push(cmd);
        }
        cmd_cv.notify_one();
    } catch (json::parse_error& e) {
        // 非JSON命令
        {
            std::lock_guard<std::mutex> lock(cmd_mutex);
            if (cmd_queue.size() >= MAX_QUEUE_SIZE) {
                cmd_queue.pop();
            }
            cmd_queue.push(cmd);
        }
        cmd_cv.notify_one();
    }
}

// 管道命令读取线程函数
static void pipe_command_reader_thread_func() {
    char buffer[1024];
    std::string accumulated_data; // 用于处理不完整的命令（断包）

    while (!should_exit) {
        if (pipe_read_fd == -1) { // 如果管道未连接，则休眠等待
            std::this_thread::sleep_for(std::chrono::seconds(1));
            continue;
        }

        ssize_t bytes_read = read(pipe_read_fd, buffer, sizeof(buffer) - 1);
        if (bytes_read > 0) {
            buffer[bytes_read] = '\0';
            accumulated_data.append(buffer); // 将新读取的数据追加到累积数据中

            size_t pos;
            // 循环处理所有完整的命令（由'\n'分隔）
            while ((pos = accumulated_data.find('\n')) != std::string::npos) {
                // 提取一个完整的命令
                std::string cmd = accumulated_data.substr(0, pos);
                // 从累积数据中移除已处理的命令和换行符
                accumulated_data.erase(0, pos + 1);

                // 去除可能存在的\r
                cmd.erase(std::remove(cmd.begin(), cmd.end(), '\r'), cmd.end());
                if (cmd.empty()) {
                    continue; // 忽略空行
                }

                // --- 从这里开始，命令处理逻辑与原来相同 ---
                if (g_is_ros_control_enabled.load()) {
                    // 控制已启用，处理命令
                    std::cout << "[管道] 接收到命令: " << cmd << " (已授权)" << std::endl;
                    
                    try {
                        json cmd_json = json::parse(cmd);
                        if ((cmd_json.contains("command") && cmd_json["command"] == "position_control") ||
                            (cmd_json.contains("type") && cmd_json["type"] == "position_control")) {
                            
                            float x = cmd_json.value("x", 0.0f);
                            float y = cmd_json.value("y", 0.0f);
                            float z = cmd_json.value("z", 0.0f);
                            float yaw = cmd_json.value("yaw", 0.0f);

                            std::cout << "[管道] 直接执行位置控制命令: x=" << x << ", y=" << y << ", z=" << z << ", yaw=" << yaw << std::endl;
                            bool result = psdk_move_by_position_offset(x, y, z, yaw);
                            
                            std::string response_str;
                            if (result) {
                                bool is_stationary = wait_until_aircraft_stationary();
                                json response = {
                                    {"status", is_stationary ? "ok" : "timeout"},
                                    {"command", "position_control"},
                                    {"message", is_stationary ? "飞行器已到达目标位置并静止" : "等待飞行器静止超时"}
                                };
                                response_str = response.dump() + "\n";
                            } else {
                                json response = {
                                    {"status", "failed"},
                                    {"command", "position_control"},
                                    {"message", "位置控制命令执行失败"}
                                };
                                response_str = response.dump() + "\n";
                            }
                            write(pipe_write_fd, response_str.c_str(), response_str.length());
                        }
                        else if (cmd_json.contains("command") && cmd_json["command"] == "vstick_advance_v") {
                            if (cmd_json.contains("data")) {
                                auto data = cmd_json["data"];
                                float right_x = data.value("left_stick_x", 0.0f);
                                float right_y = data.value("left_stick_y", 0.0f);
                                float left_x = data.value("right_stick_x", 0.0f);
                                float left_y = data.value("right_stick_y", 0.0f);
                                psdk_move_by_joystick_v(left_x, left_y, right_x, right_y);
                            }
                        }
                        else {
                            std::lock_guard<std::mutex> lock(cmd_mutex);
                            if (cmd_queue.size() >= MAX_QUEUE_SIZE) {
                                cmd_queue.pop();
                            }
                            cmd_queue.push(cmd);
                            cmd_cv.notify_one();

                            json response = {
                                {"status", "received"},
                                {"message", "Command received and queued."}
                            };
                            std::string response_str = response.dump() + "\n";
                            write(pipe_write_fd, response_str.c_str(), response_str.length());
                        }

                    } catch (json::parse_error& e) {
                        // 对于简单文本命令
                        {
                            std::lock_guard<std::mutex> lock(cmd_mutex);
                            if (cmd_queue.size() >= MAX_QUEUE_SIZE) {
                                cmd_queue.pop();
                            }
                            cmd_queue.push(cmd);
                        }
                        cmd_cv.notify_one();
                        json response = {
                            {"status", "received_text"},
                            {"message", "Text command received and queued."}
                        };
                        std::string response_str = response.dump() + "\n";
                        write(pipe_write_fd, response_str.c_str(), response_str.length());
                    }
                } else {
                    // 控制未启用，拒绝命令
                    std::cout << "[管道] 拒绝命令: " << cmd << " (未授权)" << std::endl;
                    json response = {
                        {"status", "denied"},
                        {"message", "ROS control is not enabled. Command ignored."}
                    };
                    std::string response_str = response.dump() + "\n";
                    if (pipe_write_fd != -1) {
                        write(pipe_write_fd, response_str.c_str(), response_str.length());
                    }
                }
                 // --- 命令处理逻辑结束 ---
            }
        } else if (bytes_read == 0) {
            // 对端关闭
            std::cout << "[管道] 客户端已断开连接。等待重新连接..." << std::endl;
            close(pipe_read_fd);
            close(pipe_write_fd);
            pipe_read_fd = -1;
            pipe_write_fd = -1;
            accumulated_data.clear(); // 清空累积数据
        } else if (errno != EAGAIN) {
            std::cerr << "[管道] 读取错误: " << strerror(errno) << std::endl;
            std::this_thread::sleep_for(std::chrono::seconds(1));
        }
        
        std::this_thread::sleep_for(std::chrono::milliseconds(20));
    }
}
// 管理管道连接的独立线程
static void pipe_manager_thread_func() {
    while(!should_exit) {
        // 如果管道未连接，则尝试建立连接
        if (pipe_read_fd == -1 || pipe_write_fd == -1) {
            
            // 确保旧的描述符已关闭
            if(pipe_read_fd != -1) close(pipe_read_fd);
            if(pipe_write_fd != -1) close(pipe_write_fd);
            
            // 删除旧管道文件，以防万一
            unlink(PIPE_READ_PATH);
            unlink(PIPE_WRITE_PATH);

            if (mkfifo(PIPE_READ_PATH, 0666) == -1 || mkfifo(PIPE_WRITE_PATH, 0666) == -1) {
                std::cerr << "[管道管理器] 创建管道失败，1秒后重试..." << std::endl;
                std::this_thread::sleep_for(std::chrono::seconds(1));
                continue;
            }
            
            std::cout << "[管道管理器] 等待客户端连接..." << std::endl;

            // 以阻塞方式打开，等待客户端
            pipe_read_fd = open(PIPE_READ_PATH, O_RDONLY);
            if (pipe_read_fd == -1) continue; // 循环重试

            pipe_write_fd = open(PIPE_WRITE_PATH, O_WRONLY);
            if (pipe_write_fd == -1) {
                close(pipe_read_fd);
                pipe_read_fd = -1;
                continue; // 循环重试
            }
            
            // 连接成功后，设置为非阻塞
            int flags;
            flags = fcntl(pipe_read_fd, F_GETFL, 0);
            fcntl(pipe_read_fd, F_SETFL, flags | O_NONBLOCK);
            flags = fcntl(pipe_write_fd, F_GETFL, 0);
            fcntl(pipe_write_fd, F_SETFL, flags | O_NONBLOCK);

            std::cout << "[管道管理器] 客户端已连接！" << std::endl;
        }
        
        // 已连接，休眠检查
        std::this_thread::sleep_for(std::chrono::seconds(1));
    }

    // 退出时清理
    if (pipe_read_fd != -1) close(pipe_read_fd);
    if (pipe_write_fd != -1) close(pipe_write_fd);
    unlink(PIPE_READ_PATH);
    unlink(PIPE_WRITE_PATH);
}

// 数据上报线程，同时向WebSocket和命名管道发送数据
static void data_reporting_thread_func() {
    auto last_telemetry_send = std::chrono::steady_clock::now();
    auto last_battery_send = std::chrono::steady_clock::now();

    const auto telemetry_interval = std::chrono::milliseconds(100); // 10Hz
    const auto battery_interval = std::chrono::milliseconds(2000); // 0.5Hz

    while (!should_exit) {
        auto now = std::chrono::steady_clock::now();
        bool data_written_to_pipe = false;

        // 1. 发送遥测数据 (高频)
        if (now - last_telemetry_send >= telemetry_interval) {
            last_telemetry_send = now;
            
            double lat = 0, lon = 0, alt = 0;
            float vx = 0, vy = 0, vz = 0;
            float acc_x = 0, acc_y = 0, acc_z = 0;
            float home_lat = 0, home_lon = 0;
            float q0=0,q1=0,q2=0,q3=0;
            float angular_x = 0, angular_y = 0, angular_z = 0;
            float position_x = 0, position_y = 0, position_z = 0;  // 视觉定位位置

            float position_x_latlon = 0.0, position_y_latlon = 0.0, position_z_latlon = 0.0;  // 根据经纬度获取

            psdk_get_gps_position(&lon, &lat, &alt);
            psdk_get_velocity(&vx, &vy, &vz);
            psdk_get_home_point(&home_lat, &home_lon);
            psdk_get_quaternion(&q0,&q1,&q2,&q3);
            psdk_get_acceleration(&acc_x,&acc_y,&acc_z);
            psdk_get_angular_rate_raw(&angular_x,&angular_y,&angular_z);
	        psdk_get_position_vo(&position_x, &position_y, &position_z);  // 获取视觉定位位置
            position_z = -position_z - 100;
            double speed = std::sqrt(vx * vx + vy * vy);

            latlon_to_enu_current(g_origin_lat, g_origin_lon, lat/10000000.0,lon/10000000.0,&position_x_latlon,&position_y_latlon);
            position_z_latlon = alt/1000.0;

            json telemetry_data = {
                {"data_type", "telemetry"},
                {"data", {
                    {"latitude", lat/10000000.0},
                    {"longtitude", lon/10000000.0},
                    {"altitude", 0}, //
                    {"height", alt/1000},   // 兼容字段
                    {"speed", speed},
                    {"head", calculateHeadingFromQuaternionNED(q0,q1,q2,q3)}, 
                    {"distance", 0.0},      // PSDK不易获取，用0填充
                    {"homeLocation", {{"latitude", 0}, {"longitude", 0}}},
                    {"quaternion", {{"w", q0}, {"x", q1},{"y", q2},{"z", q3}}},
                    {"velocity", {{"x", vx},{"y", vy},{"z", vz}}},
                    {"angular_velocity", {{"x", angular_x},{"y", angular_y},{"z", angular_z}}},
                    {"acceleration", {{"x", acc_x},{"y", acc_y},{"z", acc_z}}},
                    {"position_vo", {{"x", position_x},{"y", position_y},{"z", position_z}}}, // 添加视觉定位位置
                    {"position_latlon", {{"x", position_x_latlon},{"y", position_y_latlon},{"z", position_z_latlon}}}, // 根据经纬度计算当前相对坐标
                    {"origin_is_set", g_origin_is_set.load()},
                    {"origin_location", {
                        {"latitude", g_origin_is_set.load() ? g_origin_lat.load() : 0.0},
                        {"longitude", g_origin_is_set.load() ? g_origin_lon.load() : 0.0}
                    }},
                    {"ros_enabled", g_is_ros_control_enabled.load()},
                    {"ros_xyz", {{"x", g_target_x.load()},{"y",g_target_y.load()},{"z", g_target_z.load()}}}
                }}
            };

            std::string telemetry_str = telemetry_data.dump();
            // 发送到WebSocket
            websocket_client_send(telemetry_str.c_str(), telemetry_str.length());

            // 发送到命名管道
            if (pipe_write_fd != -1) {
                std::string pipe_str = telemetry_str + "\n"; // 加换行符方便外部程序按行读取
                ssize_t bytes_written = write(pipe_write_fd, pipe_str.c_str(), pipe_str.length());
                if (bytes_written < 0) {
                    // EAGAIN表示管道缓冲区已满或另一端未连接，这是非阻塞模式下的正常情况，无需报错
                    if (errno != EAGAIN) {
                        std::cerr << "写入管道失败: " << strerror(errno) << std::endl;
                    }
                } else {
                    data_written_to_pipe = true;
                }
            }
        }

        // 2. 发送电池数据 (低频)，这里是整个电池的数据，也可以获得单独电池数据
        if (now - last_battery_send >= battery_interval) {
            last_battery_send = now;

            int percent = 0;
            float voltage = 0;
            psdk_get_battery_info(&percent, &voltage);

            json battery_data = {
                {"data_type", "battery"},
                {"data", {
                    {"batteries", json::array({
                        {
                            {"percent", percent},
                            {"voltage", voltage},
                            {"connected", true}, // 连接
                            {"full_capacity", 0.0}, // 整个电池无法获取
                            {"remaining_capacity", 0.0} // 整个电池无法获取
                        }
                    })}
                }}
            };
            std::string battery_str = battery_data.dump();
            
            // 发送到WebSocket
            websocket_client_send(battery_str.c_str(), battery_str.length());

            // 发送到命名管道
            if (pipe_write_fd != -1) {
                 std::string pipe_str = battery_str + "\n";
                 ssize_t bytes_written = write(pipe_write_fd, pipe_str.c_str(), pipe_str.length());
                 if (bytes_written < 0 && errno != EAGAIN) {
                     std::cerr << "写入管道失败: " << strerror(errno) << std::endl;
                 } else {
                    data_written_to_pipe = true;
                 }
            }
        }

        // 短暂休眠，避免CPU空转
        std::this_thread::sleep_for(std::chrono::milliseconds(20));
    }
}

extern "C" {
void DjiUser_RunWebServerSample(void) {
    if (!psdk_init()) {
        std::cerr << "PSDK 初始化失败" << std::endl;
        return;
    }
    std::cout << "PSDK 初始化成功" << std::endl;

    if (!psdk_init_fc_subscription()) {
        std::cerr << "Failed to initialize flight controller subscription" << std::endl;
        return;
    }
    std::cout << "正在启动WebSocket客户端，连接到: " << SERVER_URL << std::endl;
    std::thread client_thread(websocket_client_start, SERVER_URL, handle_websocket_message);

    // 启动数据上报线程
    std::thread reporting_thread(data_reporting_thread_func);
    
    // 启动管道管理器和命令读取线程
    std::thread pipe_manager_thread(pipe_manager_thread_func);
    std::thread pipe_reader_thread(pipe_command_reader_thread_func);

    // 主线程处理命令
    while (!should_exit) {
        std::string cmd_str;
        {
            std::unique_lock<std::mutex> lock(cmd_mutex);
            // 使用带超时的等待避免CPU空转
            if (!cmd_cv.wait_for(lock, std::chrono::milliseconds(100), 
                [] { return !cmd_queue.empty() || should_exit; })) {
                continue; // 超时继续循环
            }
            
            if (should_exit) break;
            
            if (!cmd_queue.empty()) {
                cmd_str = cmd_queue.front();
                cmd_queue.pop();
            }
        }

        if (!cmd_str.empty()) {
            bool result = false;
            if (cmd_str == "enable_ros_control") {
                g_is_ros_control_enabled = true;
                std::cout << "[主控] ROS管道控制已【启用】" << std::endl;
                continue; // 处理完，跳过后续的命令解析
            } else if (cmd_str == "disable_ros_control") {
                g_is_ros_control_enabled = false;
                std::cout << "[主控] ROS管道控制已【禁用】" << std::endl;
                continue; // 处理完，跳过后续的命令解析
            }
            // 优先尝试解析为 JSON (用于虚拟摇杆)
            try {
                json cmd_json = json::parse(cmd_str);
                std::cout << cmd_json["command"] << std::endl;
                if (cmd_json.contains("command") && cmd_json["command"] == "vstick") {
                    if (cmd_json.contains("data")) {
                        auto data = cmd_json["data"];
                        float right_x = data.value("left_stick_x", 0.0f);
                        float right_y = data.value("left_stick_y", 0.0f);
                        float left_x = data.value("right_stick_x", 0.0f);
                        float left_y = data.value("right_stick_y", 0.0f);

                        // PSDK摇杆控制 (安卓的左右手柄与PSDK定义可能相反，这里按常规Mode 2映射)
                        // left_stick (x:yaw, y:throttle), right_stick (x:roll, y:pitch)
                        // psdk_move_by_joystick(yaw, throttle, roll, pitch)
                        // 假设安卓端left_stick_x/y对应yaw/throttle, right_stick_x/y对应roll/pitch
                        result = psdk_move_by_joystick(left_x, left_y, right_x, right_y);
                        // 摇杆指令通常不打印成功/失败，以避免刷屏
                        continue; // 处理完摇杆指令后继续循环
                    }
                }
                if (cmd_json.contains("command") && cmd_json["command"] == "vstick_advance_v") {
                    if (cmd_json.contains("data")) {
                        auto data = cmd_json["data"];
                        float right_x = data.value("left_stick_x", 0.0f);
                        float right_y = data.value("left_stick_y", 0.0f);
                        float left_x = data.value("right_stick_x", 0.0f);
                        float left_y = data.value("right_stick_y", 0.0f);

                        // PSDK摇杆控制 (安卓的左右手柄与PSDK定义可能相反，这里按常规Mode 2映射)
                        //yaw z y x
                        result = psdk_move_by_joystick_v(left_x, left_y, right_x, right_y);
                        // 摇杆指令通常不打印成功/失败，以避免刷屏
                        continue; // 处理完摇杆指令后继续循环
                    }
                }
                if (cmd_json.contains("command") && cmd_json["command"] == "vstick_advance_p") {
                    if (cmd_json.contains("data")) {
                        auto data = cmd_json["data"];
                        float x = data.value("left_stick_x", 0.0f);
                        float y = data.value("left_stick_y", 0.0f);
                        float yaw = data.value("right_stick_x", 0.0f);
                        float z = data.value("right_stick_y", 0.0f);
                    	result = psdk_move_by_position_offset(x, y, z, yaw);
                        continue; // 处理完摇杆指令后继续循环
                    }
                }
                if ((cmd_json.contains("command") && cmd_json["command"] == "position_control") ||
                         (cmd_json.contains("type") && cmd_json["type"] == "position_control")) {
                    float x = cmd_json.value("x", 0.0f);
                    float y = cmd_json.value("y", 0.0f);
                    float z = cmd_json.value("z", 0.0f);
                    float yaw = cmd_json.value("yaw", 0.0f);

                    std::cout << "执行位置控制命令: x=" << x << ", y=" << y << ", z=" << z << ", yaw=" << yaw << std::endl;
                    result = psdk_move_by_position_offset(x, y, z, yaw);
                    std::cout << "位置控制命令执行" << (result ? "成功" : "失败") << std::endl;
                    continue;
                }
                if (cmd_json.contains("command") && cmd_json["command"] == "set_ros_target")  {
                    float x = cmd_json.value("x", 0.0f);
                    float y = cmd_json.value("y", 0.0f);
                    float z = cmd_json.value("z", 0.0f);
                    g_target_x = x;
                    g_target_y = y;
                    g_target_z = z;
                    std::cout << "set ros: x=" << x << ", y=" << y << ", z=" << z << std::endl;
                    continue;
                }
                if (cmd_json.contains("command") && cmd_json["command"] == "set_ros_target_latlon")  {
                    float lat = cmd_json.value("lat", 0.0f);
                    float lon = cmd_json.value("lon", 0.0f);
                    float height = cmd_json.value("height", 0.0f);
                    float target_x_cal = 0, target_y_cal = 0;
                    latlon_to_enu(g_origin_lat, g_origin_lon, lat, lon,&target_x_cal,&target_y_cal);
                    g_target_x = target_x_cal;
                    g_target_y = -target_y_cal;
                    g_target_z = 0;
                    std::cout << "set ros: x=" <<target_x_cal << ", y=" << target_y_cal << ", z=" << g_target_z << std::endl;
                    continue;
                }
                if (cmd_json.contains("command") && cmd_json["command"] == "set_ros_origin")  {
                    float lat = cmd_json.value("lat", 0.0f);
                    float lon = cmd_json.value("lon", 0.0f);
                    g_origin_lat = lat;
                    g_origin_lon = lon;
                    g_origin_is_set = true;
                    continue;
                }
            } catch (json::parse_error& e) {
                // 解析失败，说明是简单文本命令，继续往下执行
            }
         // 处理RTMP直播命令
            if (cmd_str.find("start_rtmp_live ") == 0) {
                // 18 is the length of "start_rtmp_live "
                std::string url = cmd_str.substr(16); 
                std::cout << "执行开启RTMP直播命令, URL: " << url << std::endl;
                result = psdk_start_rtmp_live(url.c_str());
                std::cout << "开启RTMP直播命令执行" << (result ? "成功" : "失败") << std::endl;
            } else if (cmd_str == "stop_rtmp_live") {
                std::cout << "执行停止RTMP直播命令" << std::endl;
                result = psdk_stop_rtmp_live();
                std::cout << "停止RTMP直播命令执行" << (result ? "成功" : "失败") << std::endl;
            // 处理简单文本命令 (与安卓端命令对齐)
	        } else if (cmd_str == "takeoff") {
                std::cout << "执行起飞命令" << std::endl;
                result = psdk_takeoff();
                double current_lat, current_lon, current_alt;
                psdk_get_gps_position(&current_lon, &current_lat, &current_alt);
                g_origin_lat = current_lat/10000000.0 ;
                g_origin_lon = current_lon/10000000.0;
                g_origin_is_set = true;
                std::cout << "[坐标系] 原点已设置: Lat=" << g_origin_lat.load() << ", Lon=" << g_origin_lon.load() << std::endl;
                std::cout << "起飞命令执行" << (result ? "成功" : "失败") << std::endl;
            } else if (cmd_str == "land") {
                std::cout << "执行降落命令" << std::endl;
                result = psdk_land();
                std::cout << "降落命令执行" << (result ? "成功" : "失败") << std::endl;
            } else if (cmd_str == "confirm_land") {
                std::cout << "执行queren降落命令" << std::endl;
                result = psdk_confirm_land();
                std::cout << "降落命令执行" << (result ? "成功" : "失败") << std::endl;
            } else if (cmd_str == "go_home") {
                std::cout << "执行返航命令" << std::endl;
                result = psdk_go_home();
                std::cout << "返航命令执行" << (result ? "成功" : "失败") << std::endl;
            } else if (cmd_str == "cancel_go_home") {
                std::cout << "执行取消返航命令" << std::endl;
                result = psdk_cancel_go_home();
                std::cout << "取消返航命令执行" << (result ? "成功" : "失败") << std::endl;
            } else if (cmd_str == "enable_vstick") { // PSDK中是获取控制权
                std::cout << "获取摇杆控制权限" << std::endl;
                result = psdk_obtain_joystick_control();
                std::cout << "获取摇杆控制权限" << (result ? "成功" : "失败") << std::endl;
            } else if (cmd_str == "disable_vstick") { // PSDK中是释放控制权
                std::cout << "释放摇杆控制权限" << std::endl;
                result = psdk_release_joystick_control();
                std::cout << "释放摇杆控制权限" << (result ? "成功" : "失败") << std::endl;
            } else if (cmd_str == "psdk_gimbal_reset") {
                std::cout << "执行云台复位命令" << std::endl;
                result = DjiGimbalManager_Init();
                if (result == DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
                    result = DjiGimbalManager_Reset(E_DjiMountPosition(1), DJI_GIMBAL_RESET_MODE_PITCH_AND_YAW);
                    DjiGimbalManager_Deinit();
                }
                std::cout << "云台复位命令执行" << (result == DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS ? "成功" : "失败") << std::endl;
            } else if (cmd_str.find("psdk_gimbal_mode:") == 0) {
                // 注意: "psdk_gimbal_mode:" 是 19 个字符
                std::string mode = cmd_str.substr(19); 
                E_DjiGimbalMode gimbalMode = DJI_GIMBAL_MODE_FREE;
                
                if (mode == "yaw_follow") {
                    gimbalMode = DJI_GIMBAL_MODE_YAW_FOLLOW;
                }
                
                std::cout << "设置云台模式: " << mode << std::endl;
                result = DjiGimbalManager_Init();
                if (result == DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
                    result = DjiGimbalManager_SetMode(E_DjiMountPosition(1), gimbalMode);
                    DjiGimbalManager_Deinit();
                }
                std::cout << "设置云台模式" << (result == DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS ? "成功" : "失败") << std::endl;
            } else if (cmd_str.find("psdk_gimbal_rotate:") == 0) {
                // 注意: "psdk_gimbal_rotate:" 是 21 个字符
                std::string values = cmd_str.substr(21); 
                float pitch = 0, yaw = 0;
                
                try {
                    // 移除所有空格和不可见字符
                    values.erase(std::remove_if(values.begin(), values.end(), 
                        [](unsigned char c) { return std::isspace(c) || !std::isprint(c); }), 
                        values.end());
                    
                    size_t pos = values.find(",");
                    if (pos != std::string::npos) {
                        pitch = std::stof(values.substr(0, pos));
                        yaw = std::stof(values.substr(pos + 1));
                        
                        std::cout << "控制云台: pitch=" << pitch << ", yaw=" << yaw << std::endl;
                        
                        result = DjiGimbalManager_Init();
                        if (result == DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
                            T_DjiGimbalManagerRotation rotation = {
                                .rotationMode = DJI_GIMBAL_ROTATION_MODE_RELATIVE_ANGLE,
                                .pitch = pitch,
                                .roll = 0,
                                .yaw = yaw,
                                .time = 0.5f
                            };    
                            
                            result = DjiGimbalManager_Rotate(E_DjiMountPosition(1), rotation);
                            if (result != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
                                std::cout << "云台旋转失败，错误码: 0x" << std::hex << result << std::endl;
                            }
                            DjiGimbalManager_Deinit();
                        } else {
                            std::cout << "云台初始化失败，错误码: 0x" << std::hex << result << std::endl;
                        }
                    } else {
                        std::cout << "无效的云台控制数据格式: " << values << std::endl;
                        result = false;
                    }
                } catch (const std::exception& e) {
                    std::cerr << "解析云台控制数据失败: " << e.what() << std::endl;
                    std::cerr << "原始数据: " << values << std::endl;
                    result = false;
                }
                std::cout << "云台控制命令执行" << (result == DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS ? "成功" : "失败") << std::endl;
		}
		else {
                // std::cout << "未知或非摇杆命令: " << cmd_str << std::endl;
            }
        }
    }

    // 等待线程结束
    if (client_thread.joinable()) client_thread.join();
    if (reporting_thread.joinable()) reporting_thread.join();
    if (pipe_reader_thread.joinable()) pipe_reader_thread.join();
    
    // 清理命名管道
    close(pipe_read_fd);
    close(pipe_write_fd);
    unlink(PIPE_READ_PATH);
    unlink(PIPE_WRITE_PATH);
    
    std::cout << "客户端已关闭" << std::endl;
}

void trigger_server_exit() {
    {
        std::lock_guard<std::mutex> lock(cmd_mutex);
        should_exit = true;
    }
    cmd_cv.notify_all(); // 唤醒所有等待的线程
    websocket_client_stop();
}
}
